
#pragma once
#include "Constants.h"
#include "Hashtable.h"
#include "Operators.h"
#include <cstdlib>
#include <assert.h>
#include "stdio.h"
#include "time.h"
#include "AlgorithmInterface.h"

class AStar2D : public AlgorithmInterface
{
private:
	void reconstructGoal(int index)
	{
		int state[SIZE];
		unsigned int packedState[WORDS];

		int blank;
		int newblank;
		int tile;
		int hashval;

		while(old[index].g>0){
			blank = unpackn(old[index].ps, state);  /* unpack state and return blank position */
			newblank = old[index].oldblank;
			state[blank] = tile = state[newblank];
			state[newblank] = 0;
			packn(state,packedState);

			hashval = hash(packedState);                   /* hash the state into a bucket */
			index = search(packedState, hashval);       /* target location in hash table */
		}
	}

protected:
	Hashtable* old;				/** table of states **/
	int bucket[MAX];   /* pointer to first element in table with given hash value */
	int** open;          /* index of first element of OPEN with given F value */	
	int bestf;                                   /* F value of best nodes on OPEN */
	int bestg;									/* Maximum g value among nodes in OPEN with f=bestf (used for tie breaking) */
	int empty;                      /* index of first free location in hash table */
	int s[SIZE];                                        /* state in expanded form */
	Operators oprs[SIZE];
	short manhat [SIZE][SIZE];  /* manhattan distance of tile in a given position */
	// Solution attributes variables
	int fDiff;	/** number of steps made in this instance to get to the best next f **/
	int pregoalIndex; /** Index of parent of goal in the hashtable */
	int maxCost; /** Maximum possible cost for the used cost function. For simple A* this is maxg+maxh**/

	/**
	 Calculate the maximum cost possible
	 **/
	virtual int calculateMaxCost()
	{
		return cost(MAXH,MAXG);
	}

	/* CLOSE takes an index into the hash table and deletes it from the OPEN list,
	if its there.  
	Paramerter: int index -  index of offending element in hash table */
	virtual void close(int index)
	{
		closed++;

		int f;                                     /* f value of state to be deleted */
		int g;                                     /* g value of state to be deleted */ 
		int next;                                 /* pointer to next element in list */
		
		g=old[index].g;
		f = cost(old[index].h,g);      /* compute F value of state */
		
		if (index == open[f][g])                     /* if 1st element of its open list */
		{
			next = old[index].next;                    /* second element of open list */
			if(next!=MAX)
				old[next].prev = MAX;                        /* 2nd element will be first */
			open[f][g] = next;                          /* head points to second element */
		}
		else
		{	
			// If closing a node that is not in the top of OPEN, connect it prev and next
			if(old[index].prev!=MAX) 
			{		
				old[old[index].prev].next = old[index].next;         /* set previous to next */
				if (old[index].next != MAX) /* not 1st and no previous, closed node */
					old[old[index].next].prev = old[index].prev;        /* set next to previous */
			}
		}

		// Clear openlist pointers 
		old[index].next=MAX;
		old[index].prev=MAX;

		// Update bestf and bestg if this is the top node in OPEN
		if((bestf==f) && (bestg==g)){
			if(open[f][g]==MAX){
				// Need to search for the next best f and g
				// TODO: Implement better than linear search
				for(int newbestf=f;newbestf<maxCost;newbestf++){					
					for(int newbestg=g-1;newbestg>0;newbestg--){
						if(open[newbestf][newbestg]!=MAX){
							bestf=newbestf;
							bestg=newbestg;
							return;
						}
					}
					g=MAXG;
					fDiff++;		 /** increasing fDiff counter **/
				}
				assert("OPEN is empty");
			}
		}
	}

	/* INSERT takes a packed state, its hash value, its previous blank position, its
	g value, and its static h value, and inserts it in the OPEN list in sorted order
	using g and h. 
	Parameters:
	int ps[WORDS];                              packed state 
	int hashval;                                the hash value of the state 
	int oldblank;                               previous position of blank 
	int g;                                      distance from initial state 
	int h;                                      static heuristic value of state 
	*/
	virtual void insert(unsigned int ps[WORDS],int hashval,int oldblank,int g,int h)
	{
		int word;                                            /* word in packed state */
		int f;                                            /* combined merit function */
		int index;                           /* index in hash table where state goes */

		

		index = empty++;           /* store in first empty location and bump pointer */
		stored++;                        /* increment number of states in hash table */
	
		for (word = 0; word < WORDS; word++)        /* for each word of packed state */
			old[index].ps[word] = ps[word];       /*  store first word of packed state */

		old[index].link = bucket[hashval];      /* next pointer to old first element */
		bucket[hashval] = index;               /* new state is at head of hash chain */

		old[index].oldblank = oldblank;                /* previous position of blank */
		old[index].h = h;                               /* store new heuristic value */
		old[index].g = g;                               /* store distance from start */

		f = cost(h,g);			                  /* compute F value from G and H values */
	
		//printf("\t Generate g=%d, h=%d, f=%d, id=%d\n",g,h,f,index);

		insertToOpen(index,f,open);	/* Insert index to open with f value */	
	}

	/* INSERT the given index to the OPEN with a given cost f
	Parameters:
	int index									the index in the hashtable (old) of the new state to insert
	int f										the cost function used to sort OPEN
	int* openlist									the OPEN to insert the node nto.
	*/
	virtual void insertToOpen(int index, int f, int** openlist)
	{
		int g = old[index].g;

		// Put new node in place, tie breaking in favor of high g-value 
		old[index].prev = MAX;               /* no previous state */
		old[index].next = MAX;               /* put at head of correct open list */
		if(openlist[f][g]==MAX){ // If this is the first node with this f/g-value - insert it to its bucket
			openlist[f][g] = index;                         /* set head to point to new element */
		}
		else{ // This is not the first node with this f/g-value - insert it to its place according to tie breaking rule
			int current = openlist[f][g];
			old[index].next = current;
			old[index].prev = MAX;
			old[current].prev=index;	
			openlist[f][g] = index;
		}
	
		if (f < bestf){
			bestf = f;   /* new node better than current best, update it */
			bestg = g;
		}
		else if((f==bestf)&&(g>bestg)){
			bestg = g;
		}
	}

	/**
	 * Return the cost of a node, given its h and g
	 **/
	virtual int cost(int h, int g)
	{
		return g+h;
	}


	/* EXPAND takes the index of the best node on OPEN, unpacks it, generates each
	of its children, evaluates those that aren't on OPEN or CLOSED, and inserts them
	into OPEN. 
	Parameters:
	int index;                        hash table index of state being expanded 
	*/
	virtual int expand(int index)
	{
		int blank;                                      /* current position of blank */
		unsigned int nps[WORDS];                                  /* new state in packed form */
		int opindex;                                    /* index into operator array */
		int newindex;                                /* hashtable index of new state */
		int newblank;                                 /* blank position in new state */
		int tile;                                                /* tile being moved */
		int newh;                                 /* static h values of child states */
		int newg;                                           /* g value of new states */
		int hashval;                                  /* the hash value of the state */

		expanded++;                                    /* count total nodes expanded */
		blank = unpackn(old[index].ps, s);  /* unpack state and return blank position */
		newg = old[index].g+1;     /* g value of children is one greater than parent */
		
		// printf("Expanded f=%d, g=%d, h=%d, id=%d, exp=%d\n",newg-1+old[index].h,newg-1, old[index].h,index, expanded);
		
		for (opindex = 0; opindex < oprs[blank].num; opindex++) /* for each appl opr */
			if ((newblank = oprs[blank].pos[opindex]) != old[index].oldblank) 
			{
				s[blank] = tile = s[newblank];   /* tile moved is in new blank position */
				s[newblank] = 0;                     /* put blank in new blank position */
				generated++;                     /* increment number of nodes generated */
				newh = old[index].h + manhat[tile][blank] - manhat[tile][newblank];
			
				/* If goal has been finished - halt */
				if (newh == 0 && newg <= bestf)
					return newg;

				packn(s, nps);                         /* pack state into ps */
				hashval = hash(nps);                   /* hash the state into a bucket */
				newindex = search(nps, hashval);       /* target location in hash table */

				if (newindex == -1)                     /* state was not found in table */
					insert(nps, hashval, blank, newg, newh);     /* insert into open list */

				else
					if (old[newindex].g > newg)      /* if not in table or longer path */
					{
						close(newindex);            /* remove node from old location on open */
						reopened++;             /* keep track of how many times this happens */
						insert(nps, hashval, blank, newg, newh);   /* insert into open list */
					}

				s[newblank] = tile;          /* undo current move before doing the next */
				s[blank] = 0;                                 /* restore blank as well */
			}
		return 0;                                             /* no goal found yet */
	}

	/**
	 * Setup member variables before the search
	 **/
	virtual void setup(int initialState[SIZE])
	{
		int h;
		int blank;
		unsigned int ps[WORDS];                                       /* state in packed form */
		int hashval;                                  /* the hash value of the state */
		int maxf = calculateMaxCost();
		old = new Hashtable[MAX];

		initops();                                       /* initialize operator table */
		init(manhat);                              /* initialize evaluation function */
		fDiff = 0;	/** number of steps made in this instance to get to the best next f **/

		// Clear the hashtable
		for (int index = 0; index < MAX; index++)                    /* empty hash table */
		{
			old[index].g = -1;                  /* only g value checked for emptiness */
			old[index].next = MAX;
			old[index].prev = MAX;
			old[index].h = -1;
			bucket[index] = MAX;           /* initialize hash table buckets to empty */
		}


		// Create the open list
		open = new int*[maxf];
		for (int f = 0; f < maxf; f++){                          /* initialize OPEN lists */
			open[f]= new int[MAXG];
			for(int g=0; g<MAXG;g++){
				open[f][g] = MAX;                           /* initialize each element to NIL */
			}
		}

		// Clear state attributes counters
		generated = stored = expanded = reopened = closed = 0;    /* init statistics counters */

		// Insert initial state to OPEn
		for (blank = 0; blank < SIZE; blank++) /* for each possible position of blank*/
			if (initialState[blank] == 0) break;             /* if found, exit loop, s[blank] = 0 */
		h = manhattan(initialState);                             /* initial heuristic estimate */
		if (h == 0)
			return;                /* initial state is goal state, skip it */

		bestf = cost(h,0);                          /* compute F value of initial state */
		bestg = 0;
		empty = 0;                 /* start empty list with first location */
		packn(initialState, ps);                                         /* pack starting state */
		hashval = hash(ps);                         /* calculate correct hash bucket */
		insert (ps, hashval, SIZE, 0, h);       /* initial state, no oldblank, g = 0 */
		depth = 0;                                        /* initially no goal found */	
		fDiff = 0;
	}

	/* SEARCH takes a packed state and its hash value, and returns the index in the
	hash table where it is located. Collisions are handled by searching down the
	linked chain of the bucket. If the state is not in the table, it returns -1. 
	Parameters:
	int ps[WORDS];                                  packed state of puzzle 
	int hashval;                                   the hash value of the state 
	*/
	int search (unsigned int ps[WORDS],int hashval)
	{
		int index;                        /* index to different states in hash table */
		int pos;                            /* index to different positions in state */

		index = bucket[hashval];                     /* first state in target bucket */

		while (index != MAX)                         /* end of chain not reached yet */
		{
			for (pos = 0; pos < WORDS; pos++)           /* each position in the state */
				if (ps[pos] != old[index].ps[pos])
					break;							    /* mismatch: exit for loop */
			if (pos == WORDS)
				return (index);					/* state found, return location */
			else
				index = old[index].link;                   /* next state on chain */
		}

		return (-1);                        /* state not found, return failure flag */
	}

	/* PACKN takes a state and packs it into WORDS integers with TPW tiles per
	words. 
	Parameters:
	int s[SIZE];                                        state in expanded form
	unsigned int ps[WORDS];                             state in packed form 
	*/
	void packn (int s[SIZE],unsigned int ps[WORDS])
	{
		int pos;                               /* current position in expanded state */
		int word;                                    /* current word in packed state */

		for (word = 0; word < WORDS; word++) /* initialize packed state to all zeros */
			ps[word] = 0; 

		for (pos = 0; pos < SIZE; pos++)               /* for each position in state */
		{
			word = pos / TPW;                         /* current word of packed state */
			ps[word] = ps[word] * SIZE + s[pos];           /* add tile in base SIZE */
		}
	}

	/* UNPACKN takes a packed state of WORDS integers with TPW tiles per word and
	unpacks it into a state with SIZE tiles. It returns the position of the blank.
	Parameters:
	unsigned int ps[WORDS];                              state in packed form 
	int s[SIZE];                                         state in expanded form */
	int unpackn(unsigned int ps[WORDS],int s[SIZE])
	{
		unsigned int temp;         /* temporary copy of current word of packed state */
		int word;                                    /* current word in packed state */
		int pos;                               /* current position in expanded state */
		int blank;                                         /* position of blank tile */

		temp = ps[word = WORDS - 1];            /* make temporary copy of first word */
		for (pos = SIZE - 1; pos >= 0; pos--)              /* for each tile position */
		{
			s[pos] = temp % SIZE;                  /* tile is last digit base SIZE */
			if (s[pos] == 0)
				blank = pos;				/* record position of blank when found */
			temp = (temp - s[pos]) / SIZE;                 /* strip off last digit */
			if (pos % TPW == 0)
				temp = ps[--word];						        /* get next word */
		}
		return(blank);                             /* return position of blank tile */	
	}

	/* MANHATTAN returns the sum of the Manhattan distances of each tile, except the
	blank, from its goal position. 
	Param: s - state */
	int manhattan(int s[SIZE])
	{
		int value;                                                    /* accumulator */
		int pos;                                                    /* tile position */

		value = 0;
		for (pos = 0; pos < SIZE; pos++)
			if (s[pos] != 0)             /* blank isn't counted in Manhattan distance */
				value = value + manhat[s[pos]][pos]; /* manhattan distance of that tile */
		return(value);
	}

	/* HASH takes a packed state and returns an integer (possibly negative) which
	is its target bucket in the hash table. 
	ps - packed state of puzzle */
	long hash(unsigned int ps[WORDS])                                      
	{
		int index;                                              /* tile being hashed */
		long long int value;                                                     /* hash value */

		value = ps[0];                                             /* starting value */
		for (index = 1; index < WORDS; index++)          /* for each additional tile */
			value = value * 3 + ps[index];                   /* add new tile in base 3 */

		value = (value>=0? value : -value);  /* Implementation of ABS(int) to support long long int */

		//return(abs(value)%MAX);						  /* returned value will be mod 2 ^ word size */
		return(value % MAX);						  /* returned value will be mod 2 ^ word size */
	}
	
	/* INITOPS initializes the operator table. */
	void initops()
	{
		for (int blank = 0; blank < SIZE; blank++)  /* for each possible blank position */
		{
			oprs[blank].num = 0;                               /* no moves initially */
			if (blank > N - 1)                                       /* not top edge */
				oprs[blank].pos[oprs[blank].num++] = blank - N;       /* add a move up */
			if (blank % N > 0)                                      /* not left edge */
				oprs[blank].pos[oprs[blank].num++] = blank - 1;     /* add a move left */
			if (blank % N < N - 1)                                 /* not right edge */
				oprs[blank].pos[oprs[blank].num++] = blank + 1;    /* add a move right */
			if (blank < SIZE - N)                                 /* not bottom edge */
				oprs[blank].pos[oprs[blank].num++] = blank + N;   /* add a move down */
		}
	}

	/* INIT pre-computes the incremental evaluation function table. For a given tile
	in a given position, it calculates the Manhattan distance of the tile. 
	Params: manhat - manhattan distance of a tile, position */
	void init(short manhat[SIZE][SIZE])
	{
		int tile;                                                  /* number of tile */
		int position;                                            /* position of tile */
		pregoalIndex=-1;
		for (tile = 1; tile < SIZE; tile++)                    /* all physical tiles */
			for (position = 0; position < SIZE; position++)     /* all legal positions */
				manhat [tile][position]  = abs((tile % N) - (position % N))   
				+ abs((tile / N) - (position / N));
	}


public:

	AStar2D(void)
	{
		sprintf(algorithmName,"A*-2D");
		maxCost = calculateMaxCost();
		goalUpperbound=MAXG+1;
		desiredSuboptimality=1;
	}

	virtual ~AStar2D(void)
	{
	}

	/** 
	 * Runs the search 
	 **/
	virtual void run(int initialState[SIZE])
	{
		setup(initialState);
		expandUntilFindGoal();
	}


	/**
	 * Get the number of solution updates that occured
	 **/
	int getNumOfSolutionUpdates()
	{
		return fDiff;
	}

	/**
	 * Get the number of solution updates that occured
	 **/
	virtual bool hasMoreMemory()
	{
		if(stored < FULL)
			return true;
		else
			return false;
	}


	/**
	 * Return the quality of the found solution  
	 **/
	virtual double getSolutionQualityFactor()
	{
		if (depth>0)
			return 1;								   	   	  /** solution was found **/
		else
			return 0;									  /** solution was not found **/
	}


	/** 
	 * Clear dynamic memory 
	 **/
	virtual void clear()
	{
		delete[] old;
		for(int i=0;i<calculateMaxCost();i++){
			delete[] open[i];
		}
		delete[] open;
	}
	
	/**
	 * Return the name of the algorithm
	 **/
	virtual char* getName()
	{
		return algorithmName;
	}

	/**
	 * Continues A* expansion cycle until a goal has been found
	 **/
	virtual int expandUntilFindGoal()
	{
		int index;  /* general utility index */

		while (depth==0 && stored < FULL) /* expand until goal found or hash table full*/
		{
			if(expanded % EXPANDED_BETWEEN_PRINTS == 0)
				printf("Expanded %d nodes so far\n",expanded);
			
			index = open[bestf][bestg];                       /* new best state on open list */
			depth = expand(index);          /* expand best state, generating children */
			close(index);                           /* move best state to closed list */
		}

		// Set goal index if goal was found
		if (stored<FULL){ 
			pregoalIndex=index;
			reconstructGoal(index);
		}
		else{
			depth=MAXG+1;
		}

		return depth;
	}

};
